#include "valve_module.h"
#include "log_module.h"
#include "alarm_module.h"
#include <rtdevice.h>
#include <rthw.h>

static valve_config_t *valve_config = RT_NULL;
static rt_thread_t valve_thread = RT_NULL;
static rt_sem_t valve_sem = RT_NULL;

/* Stepper motor phase sequence for 4-phase 8-step control */
static const uint8_t step_sequence[8] = {
    0x01,  /* A1:1, A2:0, B1:0, B2:0 */
    0x03,  /* A1:1, A2:0, B1:1, B2:0 */
    0x02,  /* A1:0, A2:0, B1:1, B2:0 */
    0x06,  /* A1:0, A2:0, B1:1, B2:1 */
    0x04,  /* A1:0, A2:0, B1:0, B2:1 */
    0x0C,  /* A1:0, A2:1, B1:0, B2:1 */
    0x08,  /* A1:0, A2:1, B1:0, B2:0 */
    0x09   /* A1:1, A2:1, B1:0, B2:0 */
};

static void valve_gpio_init(void)
{
    /* Initialize coil control pins */
    rt_pin_mode(VALVE_COIL_A1_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(VALVE_COIL_A2_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(VALVE_COIL_B1_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(VALVE_COIL_B2_PIN, PIN_MODE_OUTPUT);
    
    /* Set all coils to off initially */
    rt_pin_write(VALVE_COIL_A1_PIN, PIN_LOW);
    rt_pin_write(VALVE_COIL_A2_PIN, PIN_LOW);
    rt_pin_write(VALVE_COIL_B1_PIN, PIN_LOW);
    rt_pin_write(VALVE_COIL_B2_PIN, PIN_LOW);
}

static void valve_set_phase(uint8_t phase)
{
    if (phase >= 8) return;
    
    uint8_t pattern = step_sequence[phase];
    
    /* Set coil states according to pattern */
    rt_pin_write(VALVE_COIL_A1_PIN, (pattern & 0x01) ? PIN_HIGH : PIN_LOW);
    rt_pin_write(VALVE_COIL_A2_PIN, (pattern & 0x02) ? PIN_HIGH : PIN_LOW);
    rt_pin_write(VALVE_COIL_B1_PIN, (pattern & 0x04) ? PIN_HIGH : PIN_LOW);
    rt_pin_write(VALVE_COIL_B2_PIN, (pattern & 0x08) ? PIN_HIGH : PIN_LOW);
}

static void valve_step(uint8_t direction)
{
    /* direction: 0 = backward, 1 = forward */
    if (direction) {
        valve_config->current_phase = (valve_config->current_phase + 1) % 8;
        valve_config->current_step++;
    } else {
        valve_config->current_phase = (valve_config->current_phase + 7) % 8;
        valve_config->current_step--;
    }
    
    valve_set_phase(valve_config->current_phase);
    
    /* Update position percentage */
    if (valve_config->max_steps > 0) {
        valve_config->position_percent = (valve_config->current_step * 100.0f) / valve_config->max_steps;
        
        /* Clamp to 0-100% */
        if (valve_config->position_percent < 0.0f) {
            valve_config->position_percent = 0.0f;
        } else if (valve_config->position_percent > 100.0f) {
            valve_config->position_percent = 100.0f;
        }
    }
}

static rt_err_t valve_move_to_target(uint32_t target_step, uint32_t speed)
{
    if (!valve_config->enabled) {
        return RT_ERR_IO;
    }
    
    /* Calculate direction */
    uint8_t direction = (target_step > valve_config->current_step) ? 1 : 0;
    
    /* Calculate step delay based on speed (steps per second) */
    uint32_t step_delay = (speed > 0) ? (1000 / speed) : valve_config->step_delay;
    
    valve_config->moving = RT_TRUE;
    valve_config->target_step = target_step;
    
    LOG_I("Valve moving from %d to %d, speed=%d steps/s\n", 
          valve_config->current_step, target_step, speed);
    
    /* Move step by step */
    while (valve_config->moving && valve_config->current_step != target_step) {
        valve_step(direction);
        
        /* Check if we reached target */
        if (valve_config->current_step == target_step) {
            break;
        }
        
        /* Check for stop condition */
        if (rt_sem_trytake(valve_sem) == RT_EOK) {
            valve_config->moving = RT_FALSE;
            break;
        }
        
        /* Delay between steps */
        rt_thread_mdelay(step_delay);
    }
    
    if (valve_config->moving) {
        valve_config->moving = RT_FALSE;
        LOG_I("Valve reached target position: %d steps (%.1f%%)\n", 
              valve_config->current_step, valve_config->position_percent);
    } else {
        LOG_I("Valve movement stopped at: %d steps (%.1f%%)\n", 
              valve_config->current_step, valve_config->position_percent);
    }
    
    /* Turn off coils to save power */
    valve_set_phase(0xFF);  /* All off */
    
    return RT_OK;
}

static rt_err_t valve_find_home(void)
{
    /* Simple home finding algorithm - move to limit */
    /* This would need to be implemented based on actual hardware */
    
    LOG_I("Valve homing started\n");
    
    /* Move backward until home sensor is triggered */
    valve_config->moving = RT_TRUE;
    
    for (uint32_t i = 0; i < valve_config->max_steps; i++) {
        valve_step(0);  /* Backward */
        
        /* Check home sensor (would need hardware implementation) */
        // if (home_sensor_triggered()) {
        //     break;
        // }
        
        /* Check for stop condition */
        if (rt_sem_trytake(valve_sem) == RT_EOK) {
            valve_config->moving = RT_FALSE;
            break;
        }
        
        rt_thread_mdelay(valve_config->step_delay);
    }
    
    /* Reset position to zero */
    valve_config->current_step = 0;
    valve_config->position_percent = 0.0f;
    valve_config->moving = RT_FALSE;
    
    /* Turn off coils */
    valve_set_phase(0xFF);
    
    LOG_I("Valve homing completed, position reset to 0\n");
    
    return RT_OK;
}

static void process_valve_message(message_t *msg)
{
    if (msg == RT_NULL) return;
    
    switch (msg->msg_type) {
        case MSG_CONTROL_CMD:
            if (msg->data != RT_NULL) {
                if (msg->data_len == sizeof(valve_move_absolute_t)) {
                    valve_move_absolute_t *cmd = (valve_move_absolute_t *)msg->data;
                    valve_move_absolute(cmd->target_step, cmd->speed);
                } else if (msg->data_len == sizeof(valve_move_relative_t)) {
                    valve_move_relative_t *cmd = (valve_move_relative_t *)msg->data;
                    valve_move_relative(cmd->steps, cmd->speed);
                } else {
                    /* Simple command */
                    uint8_t *cmd = (uint8_t *)msg->data;
                    switch (*cmd) {
                        case VALVE_CMD_STOP:
                            valve_stop();
                            break;
                        case VALVE_CMD_HOME:
                            valve_home();
                            break;
                    }
                }
            }
            break;
            
        default:
            break;
    }
}

rt_err_t valve_module_init(void)
{
    rt_err_t result;
    
    /* Get configuration pointer */
    valve_config = &g_system_config.valve;
    
    /* Initialize GPIO */
    valve_gpio_init();
    
    /* Create semaphore for valve control */
    valve_sem = rt_sem_create("valve_sem", 0, RT_IPC_FLAG_FIFO);
    if (valve_sem == RT_NULL) {
        LOG_E("Failed to create valve semaphore\n");
        return RT_ERR_MEM;
    }
    
    /* Create valve thread */
    valve_thread = rt_thread_create("valve", 
                                   valve_thread_entry, 
                                   RT_NULL, 
                                   1024, 
                                   15, 
                                   20);
    if (valve_thread == RT_NULL) {
        LOG_E("Failed to create valve thread\n");
        rt_sem_delete(valve_sem);
        return RT_ERR_MEM;
    }
    
    LOG_I("Valve module initialized successfully\n");
    return RT_OK;
}

void valve_thread_entry(void *parameter)
{
    message_t msg;
    
    while (1) {
        /* Wait for messages */
        if (rt_mq_recv(g_msg_queue, &msg, sizeof(msg), RT_WAITING_FOREVER) == RT_EOK) {
            if (msg.dst_module == MODULE_VALVE) {
                process_valve_message(&msg);
            }
        }
    }
}

rt_err_t valve_move_absolute(uint32_t target_step, uint32_t speed)
{
    if (target_step > valve_config->max_steps) {
        target_step = valve_config->max_steps;
    }
    
    /* Stop any current movement */
    valve_stop();
    
    /* Start movement in valve thread context */
    valve_move_absolute_t cmd = {
        .target_step = target_step,
        .speed = speed
    };
    
    return send_message(MODULE_VALVE, MSG_CONTROL_CMD, &cmd, sizeof(cmd));
}

rt_err_t valve_move_relative(int32_t steps, uint32_t speed)
{
    int32_t target_step = (int32_t)valve_config->current_step + steps;
    
    /* Clamp to valid range */
    if (target_step < 0) {
        target_step = 0;
    } else if (target_step > (int32_t)valve_config->max_steps) {
        target_step = valve_config->max_steps;
    }
    
    return valve_move_absolute((uint32_t)target_step, speed);
}

rt_err_t valve_stop(void)
{
    if (valve_config->moving) {
        /* Signal the movement loop to stop */
        rt_sem_release(valve_sem);
    }
    
    return RT_OK;
}

rt_err_t valve_home(void)
{
    /* Stop any current movement */
    valve_stop();
    
    /* Send home command */
    uint8_t cmd = VALVE_CMD_HOME;
    return send_message(MODULE_VALVE, MSG_CONTROL_CMD, &cmd, sizeof(cmd));
}

float valve_get_position_percent(void)
{
    return valve_config->position_percent;
}

uint32_t valve_get_current_step(void)
{
    return valve_config->current_step;
}

rt_bool_t valve_is_moving(void)
{
    return valve_config->moving;
}

valve_config_t *valve_get_config(void)
{
    return valve_config;
}